package fr.peltier.nicolas.robot.ai.activites;

import net.engio.mbassy.bus.MBassador;

import org.openimaj.image.processing.face.detection.DetectedFace;
import org.openimaj.math.geometry.point.Point2d;

import fr.peltier.nicolas.robot.ai.decisionnel.Contexte;
import fr.peltier.nicolas.robot.ai.memoire.ReconnaissanceFaciale;
import fr.peltier.nicolas.robot.ai.systemenerveux.event.MouvementTeteEvent;
import fr.peltier.nicolas.robot.ai.systemenerveux.event.MouvementTeteEvent.MOUVEMENTS_GAUCHE_DROITE;
import fr.peltier.nicolas.robot.ai.systemenerveux.event.MouvementTeteEvent.MOUVEMENTS_HAUT_BAS;
import fr.peltier.nicolas.robot.ai.systemenerveux.event.ReconnaissanceVocaleEvent;
import fr.peltier.nicolas.robot.ai.systemenerveux.event.RobotEvent;
import fr.peltier.nicolas.robot.ai.systemenerveux.event.VisagesEvent;

/**
 * Activité "Tracking de visage".
 * @author Nicolas Peltier (nico.peltier@gmail.com)
 */
public class TrackingActivite extends AbstractActivite {
    
    private static int LARGEUR_WEBCAM = 640;
    private static int HAUTEUR_WEBCAM = 480;

    private static int LARGEUR_ZONE_TRACKING = 180;
    private static int HAUTEUR_ZONE_TRACKING = 180;

    private static int X1 = (LARGEUR_WEBCAM - LARGEUR_ZONE_TRACKING) / 2;
    private static int X2 = X1 + LARGEUR_ZONE_TRACKING;
    private static int Y1 = (HAUTEUR_WEBCAM - HAUTEUR_ZONE_TRACKING) / 2;
    private static int Y2 = Y1 + HAUTEUR_ZONE_TRACKING;

    /**
     * Constructeur
     * @param systemeNerveux système nerveux du robot
     */
    public TrackingActivite(MBassador<RobotEvent> systemeNerveux, Contexte contexte, ReconnaissanceFaciale reconnaissanceFaciale) {
        super(systemeNerveux, contexte, reconnaissanceFaciale);
    }
    
    @Override
    public void initialiser() {
        dire("Début du suivi du visage.");
    }

    @Override
    public void handleVisagesEvent(VisagesEvent visagesEvent) {
     // Recherche du visage le plus grand
        double aireVisagePlusGrand = 0d;
        DetectedFace visagePlusGrand = null;
        for (final DetectedFace visage : visagesEvent.getListeVisages()) {
            double aireVisage = visage.getBounds().calculateArea();
            if (aireVisage > aireVisagePlusGrand) {
                aireVisagePlusGrand = aireVisage;
                visagePlusGrand = visage;
            }
        }

        final MouvementTeteEvent mouvementTeteEvent = new MouvementTeteEvent();

        if (visagePlusGrand != null) {
            // Récupération du centre de gravité du visage
            Point2d centreVisage = visagePlusGrand.getBounds().getCOG();

            // Mouvement du moteur A (lacet <==> tourner la tête horizontalement (pour faire non))
            if (centreVisage.getX() < X1 || centreVisage.getX() > X2) {
                if (centreVisage.getX() < X1) {
                    // Si le centre du visage est à gauche de la zone de tracking : tourner à gauche
                    mouvementTeteEvent.setMouvementGaucheDroite(MOUVEMENTS_GAUCHE_DROITE.TOURNER_GAUCHE);
                } else {
                    // Si le centre du visage est à droite de la zone de tracking : tourner à droite
                    mouvementTeteEvent.setMouvementGaucheDroite(MOUVEMENTS_GAUCHE_DROITE.TOURNER_DROITE);
                }
            } else {
                // Le centre du visage est dans la zone de tracking : on stoppe
                mouvementTeteEvent.setMouvementGaucheDroite(MOUVEMENTS_GAUCHE_DROITE.STOPPER);
            }

            // Mouvement du moteur B (tangage <==> tourner la tête verticalement (pour faire oui))
            if (centreVisage.getY() < Y1 || centreVisage.getY() > Y2) {
                if (centreVisage.getY() < Y1) {
                    // Si le centre du visage est au-dessus de la zone de tracking : tourner vers le haut
                    mouvementTeteEvent.setMouvementHauBas(MOUVEMENTS_HAUT_BAS.TOURNER_HAUT);
                } else {
                    // Si le centre du visage est en-dessous de la zone de tracking : tourner vers le bas
                    mouvementTeteEvent.setMouvementHauBas(MOUVEMENTS_HAUT_BAS.TOURNER_BAS);
                }
            } else {
                // Le centre du visage est dans la zone de tracking : on stoppe
                mouvementTeteEvent.setMouvementHauBas(MOUVEMENTS_HAUT_BAS.STOPPER);
            }

        } else {
            // Pas de visage : on stoppe tout
            mouvementTeteEvent.setMouvementGaucheDroite(MOUVEMENTS_GAUCHE_DROITE.STOPPER);
            mouvementTeteEvent.setMouvementHauBas(MOUVEMENTS_HAUT_BAS.STOPPER);
        }

        systemeNerveux.publish(mouvementTeteEvent);
    }

    @Override
    public void handleReconnaissanceVocalEvent(ReconnaissanceVocaleEvent reconnaissanceVocaleEvent) {
        
    }

    @Override
    public void arreter() {
        // Arrêt de la tête
        final MouvementTeteEvent mouvementTeteEvent = new MouvementTeteEvent();
        mouvementTeteEvent.setMouvementGaucheDroite(MOUVEMENTS_GAUCHE_DROITE.STOPPER);
        mouvementTeteEvent.setMouvementHauBas(MOUVEMENTS_HAUT_BAS.STOPPER);
        systemeNerveux.publish(mouvementTeteEvent);
        
        dire("Fin du suivi du visage.");
    }

}
